import os
from launch import LaunchDescription
import launch_ros
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    #     base_to_link = launch_ros.actions.Node(
    #     package='tf2_ros', 
    #     executable='static_transform_publisher', 
    #     output='screen',
    #     name='base_to_link',
    #     parameters=[{
    #         'frame_id': 'base_footprint',
    #         'child_frame_id': 'base_link',
    #         'transform': {
    #             'translation': {'x': 0.0, 'y': 0.0, 'z': 0.0},
    #             'rotation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
    #         }
    #     }]
    # )
    # base_to_gyro = launch_ros.actions.Node(
    #     package='tf2_ros', 
    #     executable='static_transform_publisher', 
    #     name='base_to_gyro',
    #     parameters=[{
    #         'frame_id': 'base_link',
    #         'child_frame_id': 'gyro_link',
    #         'transform': {
    #             'translation': {'x': 0.0, 'y': 0.0, 'z': 0.0},
    #             'rotation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
    #         }
    #     }]
    # )

    # odom_to_base = launch_ros.actions.Node(
    #     package='tf2_ros', 
    #     executable='static_transform_publisher', 
    #     name='odom_to_base',
    #     parameters=[{
    #         'frame_id': 'odom',
    #         'child_frame_id': 'base_footprint',
    #         'transform': {
    #             'translation': {'x': 0.0, 'y': 0.0, 'z': 0.0},
    #             'rotation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
    #         }
    #     }]
    # )

    # camera_to_base = launch_ros.actions.Node(
    #     package='tf2_ros', 
    #     executable='static_transform_publisher', 
    #     name='base_to_camera',
    #     parameters=[{
    #         'frame_id': 'base_link',
    #         'child_frame_id': 'camera_link',
    #         'transform': {
    #             'translation': {'x': 0.0, 'y': 0.0, 'z': 0.0},
    #             'rotation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
    #         }
    #     }]
    # )
    
    imu_config = Path(get_package_share_directory('agv_startup'), 'config', 'imu.yaml')
    witmotion_params_file = os.path.join(get_package_share_directory('agv_startup'), 'config', 'witmotion.yaml')
    # 包含 sick_scan_xd 的 launch 文件
    tim_launch_file_path = os.path.join(get_package_share_directory('agv_startup'), 'config/sick_tim_5xx.launch')
    # sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
    # tim_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_tim_5xx.launch')
    tim_top_node = Node(
        package='sick_scan_xd',
        executable='sick_generic_caller',
        output='screen',
        arguments=[
            tim_launch_file_path
        ]
    )

    # 包含 witmotion           
    witmotion_node=Node(
        package = 'witmotion_ros2',
        executable = 'witmotion_ros2',
        name='witmotion_node',
        parameters = [witmotion_params_file],
        output='screen',
        remappings=[
            ('/witmotion/imu', '/imu/data_raw')
        ]
    )
    imu_node = Node(
        package='chassis_driver',
        executable='imu_driver_node',
        name='imu_driver_node',
        parameters=[{
            'frame_id': 'imu_link',
            'topic_name': '/imu/data_raw',
            'calibration_samples': 200,
            'baud_rate': 115200,
            'port': '/dev/ttyUSB0'
        }],
        output='screen'
    )
    yesense_dir = get_package_share_directory('yesense_std_ros2')
    yesense_imu = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(yesense_dir, 'launch', 'yesense_node.launch.py')),
    )

    # 启动 chassis_driver 节点
    chassis_driver_node = Node(
        package='chassis_driver',
        executable='chassis_driver_node',
        name='chassis_driver_node',
        parameters=[{
            'odom_frame_id': 'odom',
            'robot_frame_id': 'base_footprint',
        }],
        output='screen'
    )

    # 启动 realsense 相机节点
    realsense_node = Node(
        package='realsense2_camera',
        executable='realsense2_camera_node',
        name='realsense2_camera_node',
        parameters=[{
            'enable_color': False,
            'spatial_filter.enable': True,
            'temporal_filter.enable': True
        }],
        output='screen'
    )
    base_to_link = launch_ros.actions.Node(
        package='tf2_ros', 
        executable='static_transform_publisher', 
        output='screen',
        name='base_to_link',
        arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],
    )
    base_to_imu = launch_ros.actions.Node(
        package='tf2_ros', 
        executable='static_transform_publisher', 
        name='base_to_imu',
        arguments=['0', '0', '0','0', '0','0','base_footprint','imu_link'],
    )

    odom_to_base = launch_ros.actions.Node(
        package='tf2_ros', 
        executable='static_transform_publisher', 
        name='odom_to_base',
        arguments=['0', '0', '0','0', '0','0','odom','base_footprint'],
    )

    camera_to_base = launch_ros.actions.Node(
        package='tf2_ros', 
        executable='static_transform_publisher', 
        name='base_to_camera',
        arguments=['0', '0', '0','0', '0','0','base_link','camera_link'],
    )
   
                           
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher', 
        executable='joint_state_publisher', 
        name='joint_state_publisher',
    )
    launch_dir = get_package_share_directory('agv_startup')
    realsense2_camera_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'launch', 'realsense2_camera.launch.py')),
    )

    description = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'launch','robot_mode_description.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    return LaunchDescription([
        base_to_link,
        base_to_imu,
        # camera_to_base,
        # odom_to_base,
        # witmotion_node,
        # imu_node,
        tim_top_node,
        chassis_driver_node,
        # realsense2_camera_launch,
        joint_state_publisher_node,
        # imu_filter_node,
        yesense_imu,
        description,
    ])